#include <iostream>
#include <stdexcept>
#include <signal.h>
using namespace std;
 
//#include <ssm_common.h>
//#include "ssmKinect.h"
 
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace cv;
 
#include <XnCppWrapper.h>
#include <XnOpenNI.h>
#include <XnLog.h>
using namespace xn;
//#define IMAGE_XML_PATH "./image&depthConfig.xml"

#define KINECT_IMAGE_WIDTH 640
#define KINECT_IMAGE_HEIGHT 480
#define KINECT_IMAGE_FPS 30
 
Context         g_context;
 
DepthMetaData   g_depthMD;
DepthGenerator  g_depth;
 
ImageMetaData   g_imageMD;
ImageGenerator  g_image;
 
IRMetaData      g_irMD;
IRGenerator     g_ir;
 
XnStatus        rc;
 
bool enableIR;
 
int flag_quit;
 
//-----------------------------
int connectImg();
int connectIR();
static void handler(int);
int ctrl_c();
 
int main()
{
    try {
        enableIR = false;
 
        cout << "Connecting Kinect.." << endl;
 
        if( enableIR ) {
            if( !connectIR() )
                throw runtime_error( "Connect fail: " );
        }
        else {
            if( !connectImg() )
                throw runtime_error( "Connect fail: " );
        }
        cout << "Generating image.." << endl;
 
        //------------------------------
        //rc = g_context.StartGeneratingAll();
        //if( rc != XN_STATUS_OK )
        //    throw runtime_error( "Starting Kinect error: " );
 
        //------------------------------
        //Mat  m_depth16u( 480, 640, CV_16UC1 );
        //Mat  m_image8u( 480, 640, CV_8UC3 );
        Mat  m_DepthShow( 480, 640, CV_8UC1 );
        Mat  m_ImageShow( 480, 640, CV_8UC3 );
        //Mat  m_ir16u( 480, 640, CV_16UC1 );
        Mat  m_IRShow( 480, 640, CV_8UC3 );
        unsigned char key=0;
        while(key != 27 && key != 'q' ) {
            rc = g_context.WaitAnyUpdateAll();
            if( rc != XN_STATUS_OK )
                throw runtime_error( "Read failed: " );
            if( enableIR ) {
                //cout << ".";
                g_ir.GetMetaData( g_irMD );
                g_depth.GetMetaData( g_depthMD );
                Mat m_ir16u( 480, 640, CV_16UC1, (unsigned short*)g_irMD.WritableData() );
                //Mat ir16( 480, 640, CV_16SC1, (unsigned short*)irMD.WritableData() );
                //Mat irMat;
                m_ir16u.convertTo( m_IRShow, CV_8UC3 );
                //imshow( "depth", m_DepthShow );
                imshow( "IR", m_IRShow );
            }
            else {
                //cout << "-";
                g_depth.GetMetaData( g_depthMD );
                g_image.GetMetaData( g_imageMD );
                //memcpy( m_depth16u.data, g_depthMD.Data(), 640*480*2 );
                //memcpy( m_rgb8u.data, g_imageMD.Data(), 640*480*3 );
                Mat m_depth16u( 480, 640, CV_16UC1, (unsigned short*)g_depthMD.Data() );
			    //XnUInt32 bytes = g_depthMD.BytesPerPixel();
				imwrite("imgae2.png", m_depth16u);
				
				//cout<<bytes<<endl;
                m_depth16u.convertTo( m_DepthShow, CV_8U, 255/6349.0 );
                Mat m_image8u( 480, 640, CV_8UC3, (unsigned char*)g_imageMD.Data() );
                cvtColor( m_image8u, m_ImageShow, CV_RGB2BGR );
                imshow( "depth", m_DepthShow );
                imshow( "image", m_ImageShow );
				
            }
            key = waitKey( 10 );
            switch( key ) {
                case 'm':
                    enableIR = false;
                    cout << "Reseting to produce image.." << endl;
                    if( !connectImg() )
                        throw runtime_error( "Connect fail: " );
                    break;
                case 'i':
                    enableIR = true;
                    cout << "Reseting to produce IR image.." << endl;
                    if( !connectIR() )
                        throw runtime_error( "Connect fail: " );
                    break;
            }
        }
    }
    catch( exception& ex ) {
        cout << ex.what() << xnGetStatusString( rc ) << endl << "Exiting.." << endl;
    }
    cout << endl << "Exiting.." << endl;
    g_context.StopGeneratingAll();
    g_context.Shutdown();
	
    return 0;
}
 
int connectImg() {
    try {
        g_context.StopGeneratingAll();
        g_context.Shutdown();
        //sleep(1);
        rc = g_context.Init();
        if( rc != XN_STATUS_OK )
            throw runtime_error( "Library fail: " );
        //------------------------------
        rc = g_depth.Create( g_context );
        if( rc != XN_STATUS_OK )
            throw runtime_error( "Get depth error: " );
        //------------------------------
        rc = g_image.Create( g_context );
        if( rc != XN_STATUS_OK )
            throw runtime_error( "Get image error: " );
 
        //------------------------------
        XnMapOutputMode mapMode;
        mapMode.nXRes = KINECT_IMAGE_WIDTH;
        mapMode.nYRes = KINECT_IMAGE_HEIGHT;
        mapMode.nFPS = KINECT_IMAGE_FPS;
 
        rc = g_depth.SetMapOutputMode( mapMode );
        if( rc != XN_STATUS_OK )
            throw runtime_error( "Set depth output mode error: " );
 
        rc = g_image.SetMapOutputMode( mapMode );
        if( rc != XN_STATUS_OK )
            throw runtime_error( "Set image output mode error: " );
 
        g_depth.GetMetaData( g_depthMD );
        g_image.GetMetaData( g_imageMD );
 
        if( g_imageMD.FullXRes() != g_depthMD.FullXRes() || g_imageMD.FullYRes() != g_depthMD.FullYRes() ) {
            throw runtime_error( "The device depth and image resolution must be equal!" );
        }
        if( g_imageMD.PixelFormat() != XN_PIXEL_FORMAT_RGB24 ) {
            throw runtime_error( "The device image format must be RGB24" );
        }
 
        //------------------------------
        rc = g_context.StartGeneratingAll();
        if( rc != XN_STATUS_OK )
            throw runtime_error( "Starting Kinect error: " );
    }
    catch( exception& ex ) {
        cout << ex.what() << endl;
        return 0;
    }
    return 1;
}
 
int connectIR() {
    try {
        g_context.StopGeneratingAll();
        g_context.Shutdown();
        //sleep(1);
        rc = g_context.Init();
        if( rc != XN_STATUS_OK )
            throw runtime_error( "Library fail: " );
        //------------------------------
        rc = g_depth.Create( g_context );
        if( rc != XN_STATUS_OK )
            throw runtime_error( "Get depth error: " );
        //------------------------------
        rc = g_ir.Create( g_context );
        if( rc != XN_STATUS_OK )
            throw runtime_error( "Get IR error: " );
 
        //------------------------------
        XnMapOutputMode mapMode;
        mapMode.nXRes = KINECT_IMAGE_WIDTH;
        mapMode.nYRes = KINECT_IMAGE_HEIGHT;
        mapMode.nFPS = KINECT_IMAGE_FPS;
 
        rc = g_depth.SetMapOutputMode( mapMode );
        if( rc != XN_STATUS_OK )
            throw runtime_error( "Set depth output mode error: " );
 
        rc = g_ir.SetMapOutputMode( mapMode );
        if( rc != XN_STATUS_OK )
            throw runtime_error( "Set image output mode error: " );
 
        g_depth.GetMetaData( g_depthMD );
        g_ir.GetMetaData( g_irMD );
 
        if( g_irMD.FullXRes() != g_depthMD.FullXRes() || g_irMD.FullYRes() != g_depthMD.FullYRes() ) {
            throw runtime_error( "The device depth and image resolution must be equal!" );
        }
		
        //------------------------------
        rc = g_context.StartGeneratingAll();
        if( rc != XN_STATUS_OK )
            throw runtime_error( "Starting Kinect error: " );
    }
    catch( exception& ex ) {
        cout << ex.what() << endl;
        return 0;
    }
    return 1;
}
 
static void handler(int sig){
    flag_quit = 0;
}
 
int ctrl_c(){
    static int flag_init = 0;
    if(flag_init == 0){
        signal( SIGINT, handler );
        flag_quit = 1;
    }
    flag_init = flag_quit;
 
    return flag_quit;
}

/*#include <iostream>
#include <XnOpenNI.h>
#include <XnLog.h>
#include <XnCppWrapper.h>
#include <stdlib.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <opencv/cxcore.h>

#define CHECK_RC(rc, what)                                                              \
        if (rc != XN_STATUS_OK)                                                        \
        {                                                                                       \
                printf("%s failed: %s\n", what, xnGetStatusString(rc));               \
                return rc;                                                                      \
        }

int main(void)
{
	IplImage* imgIR = cvCreateImage(cvSize(640,480), IPL_DEPTH_16U, 1);
	int k=0;	
	XnStatus nRetVal = XN_STATUS_OK;
        xn::Context context;

        nRetVal = context.Init();
        CHECK_RC(nRetVal, "Initialize context");

        xn::DepthGenerator depth;
        nRetVal = depth.Create(context);
        CHECK_RC(nRetVal, "Create depth generator");

        XnMapOutputMode dmomVGA;
        dmomVGA.nFPS = 30;
        dmomVGA.nXRes = 640;
        dmomVGA.nYRes = 480;
        XnMapOutputMode dmomQVGA;
        dmomQVGA.nFPS = 30;
        dmomQVGA.nXRes = 640;
        dmomQVGA.nYRes = 480;

        nRetVal = depth.StartGenerating();
        CHECK_RC(nRetVal, "StartGenerating() depth");

        xn::IRGenerator ir;
        nRetVal = ir.Create(context);
        CHECK_RC(nRetVal, "Create ir generator");

        xn::ImageGenerator image;
        nRetVal = image.Create(context);
        CHECK_RC(nRetVal, "Create image generator");

        nRetVal = image.StartGenerating();
        CHECK_RC(nRetVal, "StartGenerating() image");

        XnUInt32 counter = 0;
        xn::DepthMetaData depthMD;
        xn::IRMetaData IRMD;
        xn::ImageMetaData IMD;
        while (!xnOSWasKeyboardHit())
        {
			nRetVal = context.WaitOneUpdateAll(depth);
			//nRetVal = context.WaitOneUpdateAll(ir);
			if (nRetVal != XN_STATUS_OK)
			{
							printf("UpdateData failed: %s\n", xnGetStatusString(nRetVal));
							continue;
			}
			nRetVal = depth.SetMapOutputMode(dmomVGA);
			image.StopGenerating();
			depth.GetMetaData(depthMD);
            printf("DEPTH: XRes:[%d], YRes[%d], frame:[%d], Middle point is: %u.\n", depthMD.XRes(), depthMD.YRes(),depthMD.FrameID(), depthMD(depthMD.XRes() / 2, depthMD.YRes() / 2));
            nRetVal = ir.SetMapOutputMode(dmomQVGA);
            CHECK_RC(nRetVal, "Depth SetMapOutputMode XRes for 640, YRes for 480 and FPS for 30");
            ir.StartGenerating();
			ir.GetMetaData(IRMD);
            const XnIRPixel* pIRPixel = IRMD.Data();
		     printf("IR:   XRes:[%d], YRes[%d], frame:[%d], Middle point is: %d.\n", IRMD.XRes(), IRMD.YRes(), IRMD.FrameID(), pIRPixel[(IRMD.YRes() * IRMD.XRes() / 2) + (IRMD.XRes() / 2)]);
			 std::cout<<"IR2: "<<pIRPixel[(IRMD.YRes() * IRMD.XRes() / 2) + (IRMD.XRes() / 2)]<<std::endl;
			//system("PAUSE");
			//nRetVal = context.WaitOneUpdateAll(ir);
				
			 const XnIRPixel* pIrRow = IRMD.Data();
             for (XnUInt32 y = 0; y < IRMD.YRes(); y++)
             {
                     const XnIRPixel* pIR = pIrRow;
                     for (XnUInt32 x = 0; x < IRMD.XRes(); x++, pIR++)
                     {
                           ((ushort *)(imgIR->imageData + y*imgIR->widthStep))[x*imgIR->nChannels + 0] = pIrRow[x];

                     }
                     pIrRow += IRMD.XRes();
             } 
			 
			 

			cvShowImage("Window", imgIR);
			system("PAUSE");

		}
	context.Shutdown();
	return(0);
}*/